//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcMoveTask.h>
#include <rc_task/rcTaskManager_DataStruct.h>
#include <rc_move/rcmove.h>
#include <base/slog.hpp>
#include <iostream>
#include <rc_gpio/c_gpio.h>
#include <rc_gpio/soft_pwm.h>
#include <rc_message/rc_msg_server.h>
#include <rc_task/queue_sources_locks.h>
#include <vector>
namespace RC {
    namespace Task {

        void run_move_task(rc_MoveDevice rcMoveDevice) {
            using namespace RC::Message;
            slog::info << "制动任务启动中" << slog::endl;
            RobotCarMove robot;
            robot.init(rcMoveDevice);
            while (true) {
                boost::mutex::scoped_lock lock(MoveMessage::move_mutex);
                if (MessageServer::moveMessage.size_message() != 0) {
                    WHEEL_DATA wheelData = MessageServer::moveMessage.front_message();
                    robot.excute(wheelData);
                    // TODO:这个POP要不要存在争议
                    MessageServer::moveMessage.pop_message();
                    sleep(0.01);
                }
            }
//            robot.start();
            slog::info << "制动任务启动完成" << slog::endl;
        }
    }
}
